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ABSTRACT 

This  article  presents  a  model  predictive  control  based 
obstacle  avoidance  algorithm  for  autonomous  ground  vehicles 
in  unstructured  environments.  The  novelty  of  the  algorithm  is  the 
simultaneous  optimization  of  speed  and  steering  without  a  priori 
knowledge  about  the  obstacles.  Obstacles  are  detected  using  a 
planar  LIDAR  sensor  and  a  multi-phase  optimal  control  problem 
is  formulated  to  optimize  the  speed  and  steering  commands 
within  the  detection  range.  Acceleration  capability  of  the  vehicle 
as  a  function  of  speed,  and  stability  and  handling  concerns 
such  as  tire  lift-off  are  taken  into  account  as  constraints  in  the 
optimization  problem,  whereas  the  cost  function  is  formulated  to 
navigate  the  vehicle  as  quickly  as  possible  with  smooth  control 
commands.  Thus,  a  safe  and  quick  navigation  is  enabled  without 
the  need  for  a  preloaded  map  of  the  environment.  Simulation 
results  show  that  the  proposed  algorithm  is  capable  of  navigating 
the  vehicle  through  obstacle  fields  that  cannot  be  cleared  with 
steering  control  alone. 
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1  INTRODUCTION 

Obstacle  avoidance  is  a  critical  capability  for  autonomous 
ground  vehicles  (AGVs).  It  refers  to  the  task  of  sensing  the 
vehicle’s  surroundings  and  generating  control  commands  to 
navigate  the  vehicle  safely  around  the  obstacles.  Typically, 
safety  is  interpreted  as  collision  free,  which  may  be  an  adequate 
interpretation  in  certain  applications  such  as  small  ground 
robots.  However,  for  the  AGVs  that  are  at  least  the  size  of 
a  passenger  vehicle,  vehicle  dynamics  related  safety  concerns 
such  as  excessive  sideslip  or  tire  lift-off  are  also  important. 
Thus,  obstacle  avoidance  algorithms  are  needed  that  can  utilize 
knowledge  of  vehicle  dynamics  to  avoid  collisions  even  when  the 
vehicle  is  operated  at  its  limits.  Creating  such  algorithms  would 
not  only  be  beneficial  for  safe  and  fast  navigation  of  AGVs, 
thereby  improving  their  performance,  but  they  could  also  be  used 
as  an  advanced  safety  feature  in  human  driven  vehicles. 

Many  obstacle  avoidance  algorithms  have  been  developed 
in  the  literature  that  allow  for  fast,  continuous,  and  smooth 
motion  of  AGVs  among  unexpected  obstacles.  The  can  be 
classified  into  four  categories:  graph  search  based  methods  [1] 

[2] ,  virtual  potential  and  navigation  function  based  methods 

[3]  [4],  meta-heuristic  based  methods  [5],  and  mathematical 
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optimization  based  methods  [6]  [7].  Among  these  categories, 
mathematical  optimization  based  methods  are  particularly 
attractive,  because  they  offer  a  rigorous  and  systematic  approach 
to  take  vehicle  dynamics  and  safety  constraints  into  account.  The 
model  predictive  control  (MPC)  approach  is  a  widely  adopted 
mathematical  optimization  based  approach.  Prior  research 
has  demonstrated  successful  applications  of  MPC  to  obstacle 
avoidance  in  AGVs  [8],  [9],  [10],  [11],  [12],  [13],  Some  active 
safety  methods  leverage  the  MPC  framework,  as  well,  to  ensure, 
for  example,  safe  lane  keeping  or  vehicle  stability  [14],  [15], 
[16],  [17],  The  first  applications  of  MPC  to  obstacle  avoidance 
in  AGVs  assumed  that  the  controller  has  full  knowledge  about 
the  environment.  They  also  were  not  concerned  with  the 
level  of  fidelity  that  the  model  used  by  the  controller  needs 
to  possess  for  satisfactory  performance,  where  the  performance 
criteria  in  some  cases  also  include  the  dynamical  safety  of  the 
vehicle,  such  as  no  tire  lift-off.  Prior  work  by  the  authors 
developed  a  nonlinear  MPC  algorithm  with  a  corresponding 
sensor  data  processing  algorithm  for  obstacle  avoidance  in 
high-speed,  large-size  autonomous  ground  vehicles  that  perceive 
the  environment  only  through  a  planar  LIDAR  sensor  [18],  [19], 
The  algorithm  can  navigate  an  AGV  in  an  obstacle  field  without 
collision  while  ensuring  vehicle  dynamical  safety.  However,  the 
formulation  assumes  that  the  vehicle  speed  is  constant,  which 
can  limit  the  mobiltiy  performance  and  the  obstacle  fields  that 
can  be  cleared  with  this  algorithm. 

This  paper  presents  a  novel  MPC  formulation  that 
simultaneously  optimizes  both  the  longitudinal  speed  and 
steering  control  commands  in  an  AGV  that  relies  only  on  a  planar 
LIDAR  sensor  to  detect  the  obstacles.  A  typical  military  truck 
is  considered  as  a  reprentative  high-speed,  large-size  AGV  with 
significant  vehicle  dynamics.  No  map  of  the  environment  is 
available  to  the  algorithm,  but  the  target  location  is  assumed  to 
be  known.  Given  the  obstacle  boundaries  that  are  detected  within 
the  LIDAR  range  and  the  vehicle’s  dynamic  and  mechanical 
constraints,  the  algorithm  aims  to  navigate  the  vehicle  as  quickly 
as  possible  towards  the  target  while  ensuring  safety.  Vehicle 
dynamics  are  taken  into  account  through  a  3  degrees  of  freedom 
(DoF)  vehicle  model  in  the  algorithm.  Vehicle  speed  and 
acceleration  limits  are  generated  based  on  a  powertrain  model. 
For  the  particular  vehicle  of  interest,  the  vehicle  dynamical 
safety  requirement  is  translated  to  avoiding  single  tire  lift-off, 
because  tire  lift-off  will  happen  before  excessive  sideslip.  A 
constrained  optimal  control  problem  (OCP)  is  formulated  and 
solved  within  the  MPC  framework.  Simulations  with  a  14  DoF 
vehicle  model  as  the  plant  are  given  to  highlight  the  benefits  of 
the  new  framework  compared  to  prior  work. 

The  rest  of  the  paper  is  organized  as  follows.  Section  2  gives 
an  overview  of  the  nonlinear  MPC -based  obstacle  avoidance 
algorithm.  Section  3  presents  the  OCP  formulation  in  the  MPC 
in  detail  and  outlines  the  solution  strategy.  Section  4  presents 
and  discusses  the  simulation  results.  Conclusions  are  drawn  in 


Section  5. 

2  OVERVIEW  OF  THE  MPC-BASED  OBSTACLE 
AVOIDANCE  FRAMEWORK 

Fig.  1  shows  the  schematic  of  the  nonlinear  MPC  algorithm  with 
the  AGV  in  closed  loop.  This  section  gives  an  overview  of  this 
framework  and  explains  its  basic  flow  at  a  high  level. 

The  problem  of  interest  to  be  solved  with  the  MPC 
framework  can  be  summarized  as  follows.  Consider  an  AGV 
in  an  unstructured  environment  as  illustrated  in  Fig.  2.  Here  the 
term  ’’unstructred  environment”  means  that  there  are  no  lanes  to 
follow  and  no  traffic  rules  to  obey.  The  mission  of  the  AGV  is 
to  move  from  its  initial  position  to  a  given  goal  position  safely 
and  as  quickly  as  possible.  Between  the  two  positions  there 
exist  obstacles,  whose  location,  size,  and  shape  are  not  known  a 
priori.  Obstacle  information  is  obtained  through  a  planar  LIDAR 
sensor,  which  is  mounted  in  front  of  the  vehicle  and  returns  the 
distance  to  the  closest  obstacle  boundary  in  each  radial  direction 
at  an  angular  resolution  of  e.  Fig.  2  illustrates  a  case  with  three 
obstacles.  The  vehicle  is  assumed  to  travel  on  a  planar  surface; 
hence,  a  2D  representation  as  shown  in  Fig.  2b  suffices  for  the 
purposes  of  this  work. 

The  task  of  the  MPC  algorithm  is  to  use  the  LIDAR 
data  to  nagivate  the  vehicle  through  the  obstacle  field  safely 


FIGURE  1.  Schematic  of  the  MPC  based  obstacle  avoidance 
algorithm. 
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and  quickly.  Two  safety  constraints  are  considered;  namely, 
avoiding  the  obstacles,  and  ensuring  the  vehicle’s  dynamical 
safety.  The  first  safety  constraint  is  fulfilled  by  constraining 
the  position  of  the  AGV  inside  the  safe  region  established  from 
the  LIDAR  data.  The  data  from  LIDAR  is  first  processed  by 
simplifying  the  obstacle  shape  and  adding  safety  margins.  As 
shown  by  the  example  safe  region  in  Fig.  2b,  which  is  the  shaded 
area,  it  is  typically  not  possible  to  define  the  safe  area  using 
a  single  differentiable  function.  This  is  problematic,  because 
the  OCP  solver  requires  all  functions  to  be  twice  continuously 
differentiable.  To  address  this  challenge,  the  safe  region  is 
partitioned  into  several  sub-regions  and  a  multi-phase  OCP 
formulation  is  used.  The  procedures  of  partitioning  and  the  usage 
of  sub-regions  are  described  in  detail  in  [19].  Only  the  basic 
ideas  are  summarized  here  for  brevity. 

Assuming  the  edges  of  the  obstacles  are  straight,  the  safe 
region  can  be  partitioned  in  a  way  such  that  each  sub-region  can 
be  specified  by  a  set  of  linear  inequalities.  Many  partitioning 
options  exist  and  Fig.  3  illustrates  one  way  of  partitioning  the 
safe  region  shown  in  Fig.  2b.  In  this  approach,  the  safe  area 
is  divided  into  sectors  and  triangles,  where  sectors  are  called 
openings  and  they  are  free  from  obstacles,  whereas  triangles  are 
regions  terminated  by  obstacle  boundaries.  To  avoid  obstacles 
and  move  towards  the  target,  the  first  sub-region  to  transverse 
is  the  sub-region  in  front  of  the  vehicle  (OB4),  and  the  last 
sub-region  to  transverse  is  one  of  the  openings.  Note,  however, 
that  because  of  the  limited  turning  radius  of  the  vehicle,  not 
all  openings  are  necessarily  feasible.  For  example,  in  Fig.  3, 
there  are  two  feasible  openings,  OP2  and  OP3,  whereas  OP1  and 
OP4  are  infeasible  opening,  because  the  vehicle  cannot  steer  into 
these  sub-regions  without  moving  outside  the  safe  region.  Thus, 
there  are  two  possible  sub-region  sequences  the  vehicle  can  move 
through:  OB4  — >  OP2  and  OB4  OP3.  Based  on  the  location  of 
the  goal  position,  one  or  both  of  the  sub-region  sequences  need 
to  be  evaluated.  Thus,  at  each  step  of  the  MPC,  a  multi-phase 
OCP  is  formulated  for  each  of  the  sequences  to  be  evaluated. 
The  OCP  is  multi-phase  because  the  optimal  trajectory  typically 
needs  to  traverse  multiple  sub-regions  and  hence  the  position 
constraints  are  different  for  each  sub-region.  Fig.  4  shows  the 
optimal  trajectory  for  the  sequence  OB4  — ►  OP3  as  an  example. 
The  optimal  trajectory  for  the  sequence  OB4  — ►  OP2  is  obtained 
similarly.  These  two  optimal  trajectories  are  then  compared  and 
the  control  commands  corresponding  to  the  better  one  is  executed 
by  the  AGV. 

The  second  safety  constraint  is  ensuring  dynamical  safety, 
which  is  defined  as  avoiding  single  tire  lift-off.  The  details  of 
this  constraint  are  discussed  in  Section  3. 

3  OPTIMAL  CONTROL  PROBLEM  FORMULATION 

This  section  presents  the  main  contribution  of  this  paper;  namely, 
the  multi-phase  OCP  formulation  that  needs  to  be  solved  at  each 


step  of  the  MPC  to  simultaneously  optimize  the  longitudinal 
speed  and  steering  angle. 

The  formulation  in  general  form  is  given  by 


minimize 


subject  to 
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By  minimizing  the  cost  function  specified  in  Eq.  (1),  subject 
to  constraints  defined  in  Eq.  (2)  -  Eq.  (13)  for  all  phases, 
the  optimal  state  trajectories  G  [Tl~l,Tl],  the  optimal 

control  trajectories  G  \Tl~x ,Tl],  and  the  times  T'*1,  T\ 

i  =  1 ....  .A  for  transitioning  from  one  sub-region  to  the  next  are 
obtained,  where  N  is  the  total  number  of  phases;  i.e.,  the  number 
of  sub-regions  in  a  given  sequence.  The  following  sub-sections 
define  the  variables  and  explain  the  problem  formulation  in 
detail.  The  constraints  are  discussed  first,  and  the  cost  function 
formulation  is  explained  next. 


3.1  Eq.  (2):  Vehicle  model 

A  3  DoF  vehicle  model  is  used  in  the  MPC  to  predict  the 
dynamics  of  the  AGV.  As  shown  in  [19],  this  model  is  sufficient 
for  the  purposes  of  MPC  when  the  longitudinal  load  transfer 
and  tire  nonlinearities  are  taken  into  account.  The  state-space 
equation  for  the  3  DoF  nonlinear  vehicle  model  can  be  written  as 

|=^)+J?C  (14) 
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(a)  A  sample  obstacle  field  and  the 
LIDAR  detection  data  plotted  in  the 
3D  space 


(b)  Top  view  of  the  LIDAR  data  and 
the  safe  area 


FIGURE  2.  A  sample  obstacle  field  and  the  output  of  LIDAR. 


FIGURE  3.  Exemplified  partition  of  the  safe  region. 


FIGURE  4.  An  example  of  sub-region  sequence  and  the 
corresponding  optimal  trajectory. 
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where  the  state  vector  is  q 1  =  [xy  yr  U  V  r  8j  ax  ]  and  the 
control  vector  is  =  [gy  Jx],  As  shown  in  Fig.  5,  (x,y)  is  the 
vehicle’s  front  center  position  in  global  coordinates;  y/  is  yaw 
angle;  U,  and  V  are  longitudinal  and  lateral  speeds  in  vehicle 


fixed  coordinate,  respectively;  r  is  yaw  rate;  8/  is  steering  angle; 
ax  is  longitudinal  acceleration;  is  steering  rate;  and  Jx  is 
longitudinal  jerk.  As  for  the  vehicle  parameters,  M  is  vehicle 
mass;  Izz  is  moment  of  inertia;  and  Lf  and  L,  are  distances 
between  the  vehicle’s  center  of  gravity  location  and  the  front  axle 
and  rear  axle,  respectively.  Fyj  and  Fy  r  are  tire  lateral  forces 
generated  at  the  front  axle  and  the  rear  axle,  respectively.  They 
are  predicted  using  the  the  pure-slip  Pacejka  Magic  Formula  tire 
model  [20],  and  are  functions  of  corresponding  tire  vertical  loads 
and  slip  angles  as  shown  in  Fig.  6. 

In  the  tire  model,  tire  vertical  loads  are  required  as  inputs. 
The  vertical  loads  acting  on  the  four  wheels  are  given  by  the 


following  equations  [2 1  ] 

FzJi  =  \  (FzJ o  -  A FZrX)  -  A Fzjf  (15) 

Fzjr  =  3  (FzJ0  ~  A FZrX)  +  A Fzjf  (16) 

Fz,ri  =  3  {Fz,r0  +  A FZ}X)  -  A Fz>yr  ( 17) 

Fz,rr  =  3  (7v,r0  +  AF;  v)  +AF’;i),r  (18) 


where  Fzj o  =  ^  +8  js  the  static  front  axle  load;  FzrQ  =  L  ^  is 
the  static  rear  axle  load;  A FZJX  is  the  longitudinal  load  transfer; 
AF-  yf  is  the  front  axle  lateral  load  transfer;  and  AF-yr  is  the  rear 
axle  lateral  load  transfer. 

These  load  transfers  are  approximated  by  the  following 
relationships 


A/7  a-  p 

a  KZrX  (U  —  Vr) 

(19) 

A Fz<yf  - 

a  Kz,yf  (y  +  Ur) 

(20) 

A FZtyr  ? 

a  Kz,yr  {y  +  Ur) 

(21) 

where  Kzx  is  defined  as  the  longitudinal  load  transfer  coefficient; 
and  Kzyj  and  Kzyr  are  defined  as  the  front  and  rear  lateral 


FIGURE  5.  Schematic  of  the  3  DoF  vehicle  model. 


FIGURE  6.  Lateral  tire  forces  described  by  the  Pacejka  tire  model. 
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load  transfer  coefficients,  respectively.  These  coefficients  are 
obtained  from  several  sets  of  simulations  with  a  14  DoF  vehicle 
model  [22]  that  includes  suspension  dynamics  [22],  nonlinear 
tire  dynamics  [20],  powertrain  dynamics  [23]  [24],  and  brake 
dynamics  [25].  The  details  of  the  complete  model  is  omitted 
here  for  brevity,  but  can  be  found  in  the  referred  papers. 

Fig.  7  and  Fig.  8  show  the  simulation  results  for  the 
longitudinal  and  lateral  load  transfers  with  the  14  DoF  model 
and  the  straight  line  fittings  whose  slopes  represent  the  constant 
load  transfer  coefficients. 

To  obtain  the  longitudinal  load  transfer  coefficient,  two  sets 
of  simulations  are  conducted.  In  the  first  set  of  simulations, 
the  throttle  command  is  maintained  constant  at  different  levels 
with  zero  braking  command  and  zero  steering  angle.  The  results 
are  used  to  study  the  longitudinal  load  transfer  during  vehicle 
acceleration.  In  the  second  set  of  simulations,  the  braking 
command  is  maintained  constant  at  different  levels  with  zero 
throttle  command  and  zero  steering  angle.  The  results  can  then 
be  used  to  study  the  longitudinal  load  transfer  during  vehicle 
deceleration.  The  black  dashed  line  in  Fig.  7  is  a  line  that  passes 
through  the  origin  with  a  slope  estimated  from  these  two  sets  of 
data.  The  deviation  from  this  line  to  the  upper  left  side  is  caused 
by  aerodynamic  drag. 

The  data  for  estimating  the  lateral  load  transfer  coefficients 
is  generated  by  following  the  maneuver  specified  by  Fig.  9a  and 
Fig.  9b.  The  vehicle  steers  with  a  sinusoidal  angle.  In  the 
meantime,  the  speed  is  changing  at  a  slower  frequency.  Fig.  8 
shows  the  simulation  data  points  and  the  fitted  lines.  Simulations 
with  other  maneuvers  confirm  the  results. 

Even  through  the  results  from  the  simulations  are  not 
perfectly  affine,  the  approximations  in  Eq.  (19)-(21)  are 
sufficient  for  the  purposes  of  this  work  as  the  approximation  error 
introduced  by  these  estimated  coefficients  is  on  the  order  of  5% 
as  the  comparative  simulation  results  in  Fig.  9  illustrate. 

3.2  Eq.  (3):  Initialization 

The  initial  states  of  each  phase  are  set  the  same  as  the  final  states 
of  the  previous  phase.  For  the  first  phase,  the  initial  values  of  the 
states  x,y,  y/,V,  and  r  are  from  the  measurements.  To  maintain 


FIGURE  7.  Longitudinal  load  transfer  as  a  function  of  longitudinal 
acceleration. 


a  smooth  reference  speed  and  a  smooth  steering  sequence,  the 
initial  values  of  U,ax,  and  5/  are  the  values  from  the  end  of  the 
execution  horizon  of  the  previous  step. 

3.3  Eq.  (4):  Position  constraints 

Vehicle  obstacle  avoidance  is  enforced  through  position 
constraints;  the  vehicle  trajectory  must  stay  within  the  safe 
region  that  is  obtained  after  the  LIDAR  data  is  processed.  For 
each  of  the  phase  in  the  OCP,  a  set  of  position  constraints 
compacted  in  the  following  form  is  applied 

<  ci°xl,f  €  [r-\r]  (22) 

where  j,  and  C^x]  are  vectors  of  length  L,  which  is 

the  total  number  of  lines  bounding  the  sub-region  in  phase  i.  The 


FIGURE  8.  Front  and  rear  lateral  load  transfers  as  a  function  of  lateral 
acceleration,  respectively. 


(a)  Longitudinal  speed  profile. 


(c)  Comparison  of  the  actual  and 
the  predicted  vertical  loads. 


(b)  Steering  angle  profile. 


-0.2  - 

0  50  100 


<[s] 

(d)  Difference  of  the  actual  and 
the  predicted  vertical  loads. 


FIGURE  9.  A  sample  simulation  used  to  validate  the  accuracy  of 
vertical  load  prediction.  The  vehicle  performs  the  maneuver  specified 
by  subplots  (a)  and  (b).  The  blue  solid  line  in  subplot  (c)  is  the  vertical 
load  of  rear  left  tire  from  the  14  DoF  vehicle  model.  The  red  dashed 
line  is  the  prediction  using  Eq.  (17)  with  state  values  from  the  14  DoF 
vehicle  model  simulation. 
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entries  of  these  vectors  are  the  outputs  of  LIDAR  data  processor. 

3.4  Eq.  (5):  Dynamical  safety  constraints 

In  this  study,  ensuring  the  vehicle’s  dynamical  safety  is  translated 
to  avoiding  single  tire  lift-off.  This  is  a  conservative  criterion 
used  to  prevent  rollover  [26].  Prior  work  enforced  this  constraint 
through  steering  angle  bounds  [7],  or  lateral  acceleration  bounds 
[27].  However,  these  approaches  ignore  the  effect  of  longitudinal 
acceleration,  which  is  an  important  factor  to  consider  in  the 
variable  speed  case.  Hence,  in  this  work,  the  no  tire  lift-off 
requirement  is  taken  into  account  directly  by  enforcing  a  positive 
vertical  load  on  all  four  tires  at  all  times.  Because  Fzjq  >  Fz>ro 
and  Kzyr  >  Kz  yf,  vertical  loads  of  the  two  tires  on  the  back  are 
constrained  to  be  greater  than  a  positive  threshold  value.  In 
Fig.  10,  the  range  of  vehicle  accelerations  for  limiting  rear  tire 
vertical  loads  is  smaller  than  the  range  for  limiting  front  tire 
vertical  loads.  Thus,  it  is  sufficient  to  include  only  the  vertical 
load  limits  of  the  two  rear  tires. 


FIGURE  10.  When  the  vehicle  accelerations  are  within  the  entire 
colored  region,  the  vertical  loads  of  the  two  front  tires  will  be  greater 
than  A, threshold  =  1000  N.  However,  to  make  sure  that  the  vertical 
loads  of  the  two  rear  tires  are  greater  than  this  threshold,  the  vehicle 
accelerations  need  to  stay  within  the  blue  region. 


This  constraint  can  be  expressed  using  the  following 
inequalities,  which  are  obtained  by  substituting  Eq.  (19),  and 
Eq.  (21)  into  Eq.  (17),  and  Eq.  (18): 


:,yr  (V(0  (0  +  U(i)  (0)  >  ^threshold 


(23) 


Another  important  vehicle  dynamics  related  safety  concern 
is  excessive  sideslip,  which  is  considered  as  a  handling 
instability.  To  prevent  it,  methods  such  as  using  a  yaw-sideslip 
state  envelope  [17]  could  be  employed.  However,  the  vehicle 
platform  of  interest  in  this  work  has  a  relatively  higher  center  of 
gravity  and  thus  tire  lift-off  occurs  before  excessive  sideslip. 


Full  braking  command 


(b)  Longitudinal  acceleration. 

FIGURE  11.  Vehicle  longitudinal  speed  and  acceleration  profiles  for 
a  full  throttle  -  full  brake  cycle. 

3.5  Eq.  (6)  -  Eq.  (10):  State  and  Control  Bounds 

Constant  bounds  are  imposed  on  the  steering  angle  and  steering 
rate  in  Eq.  (6),  and  Eq.  (7),  respectively.  These  bounds  reflect 
the  vehicle  mechanical  limits  and  steering  actuator  performance 
limits. 


The  vehicle  speed,  acceleration,  jerk  bounds  in  Eq.  (8), 
Eq.  (9),  and  Eq.  (10)  are  associated  with  the  powertrain  and  brake 
dynamics.  Powertrain  dynamics  are  modeled  according  to  [23] 
[24].  The  model  includes  the  flywheel,  engine,  torque  converter, 
transmission,  and  differential.  The  dynamics  of  the  hydraulically 
actuated  brake  is  modeled  using  the  single  state  model  in  [25] 
that  produces  a  good  representation  of  the  dynamics  from  the 
master  cylinder  to  vehicle  deceleration.  Fig.  11a  and  Fig.  1  lb  are 
longitudinal  speed  and  acceleration  profiles,  respectively,  when 
a  full  throttle  command  is  applied  followed  by  a  full  braking 
command.  As  a  result  of  this  simulation,  the  maximum  speed 
is  set  as  30  m/s  in  the  OCP  formulation.  Furthermore,  as  shown 
in  Fig.  12,  acceleration  capability  depends  on  the  instantaneous 
speed.  The  upper  bound  and  lower  bound  are  approximated 
using  fourth  order  polynomials  as  follows: 

<A,max  (u)  =  C\U2  +C2U"  +  C3U  T  C4  (24) 

a.T.min  {U)  =  c5U 3  +  c6U2  +  cqU  +  c8  (25) 

where,  ci,---,C8  are  parameters  obtained  from  polynomial 
fitting  to  the  simulation  data. 
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Speed  [m/s] 

FIGURE  12.  Vehicle  acceleration  limits  as  a  function  of  vehicle 
longitudinal  speed. 

3.6  Eq.  (11)  -  Eq.  (13):  Terminal  constraints 

The  first  terminal  constraint,  Eq.  (11),  limits  the  vehicle  speed 
at  the  end  of  the  prediction  horizon.  This  limit  is  introduced 
to  prepare  for  potential  obstacles  in  the  future.  Specifically,  the 
vehicle  is  allowed  to  accelerate  at  the  beginning  of  the  prediction 
horizon,  but  then  is  required  to  decelerate  to  a  threshold  speed  at 
the  end  of  the  prediction  horizon,  since  no  obstacle  information 
such  as  location,  shape,  and  size  are  known  a  priori.  The  second 
terminal  constraint,  Eq.  (12),  constrains  the  vehicle  trajectory 
to  be  within  the  LIDAR  detection  range.  The  third  terminal 
constraint,  Eq.  (13),  specifies  the  limits  on  the  prediction  horizon. 

3.7  Eq.  (1):  Cost  function 

The  cost  function  defines  in  what  sense  a  trajectory  is  considered 
to  be  optimal.  In  this  work,  the  cost  function  formulation 
includes  six  terms  that  are  linearly  combined  using  relative 
weights  as  follows: 

S t  2 

J  — - b  WyYfcff  +  WtTp  +W/j/i  +  W/2/2  +  W/j/3  (26) 

SO 

where  so  is  the  distance  between  the  prediction  initial  position 
[x(0),y(0)]  and  the  goal  position  [xg,yg],  whereas  st  is  the 
distance  between  the  prediction  final  position  [x(Tp),y(Tp)\  and 
[x Visual  representations  of  these  variables  are  shown  in 
Fig.  2b.  y/diff  is  the  difference  between  the  final  heading  angle 
y/(Tp)  and  the  angle  of  the  [xg,yg]  relative  to  [x(Tp),y(Tp)\,  which 
is  calculated  as 

V'diff  =  atan2  [sin(v/(7’p)  -  v/frg),cos(v/(rp)  -  y/frg)]  (27) 
y/frg  =  atan2  \yg-y(Tp),xg-x(Tp)}  (28) 

Tp  is  the  prediction  horizon.  If  proper  weighting  factors  are 
selected,  the  first  three  terms  will  result  in  a  trajectory  in  which 
the  end  point  of  the  predicted  trajectory  is  close  to  the  target,  the 
final  heading  angle  is  pointing  to  the  target,  and  the  time  used  to 
cover  the  prediction  distance  is  small. 

Furthermore,  three  integral  terms  are  included  in  the  cost 
function.  The  first  term,  I\ ,  penalizes  the  cost  when  the  tire 
vertical  load  is  close  to  the  specified  threshold,  which  is  used 
to  prevent  vehicle  from  operating  at  the  limit  unnecessarily. 
The  second  term,  h.  is  used  to  minimize  the  integral  over  the 


prediction  horizon  of  the  distance  to  the  line  that  is  passing 
through  the  goal  [xg.yg]  along  a  desired  direction  tj)g.  This  term  is 
used  to  have  the  vehicle  pass  through  the  target  from  the  desired 
direction.  The  third  term,  Ij,  is  a  regularization  term  minimizing 
the  control  effort  that  is  defined  as  the  integral  of  the  weighted 
sum  of  8j,  qj,  and  j\. 

l\  =J  {2  +  tanh  [—  —  a)/b]  +tanh[—  (FZtrr  —  a)/b]}  df 

(29) 

h  =  Jq  '  {sin(0g)[x(f)-xg]-cos(0g)[y(f)-yg]}2dr  (30) 

Ii  =  fQ  '  [$}(*) +ws$2f(t)  +  wjJl(t)]  dr  (31) 

This  completes  the  description  of  the  formulation. 

3.8  Solving  the  OCP 

The  resulting  OCP  problem  is  solved  using  a  pseudo-spectral 
method  to  transform  the  continuous-time  optimization  problem 
into  a  nonlinear  programming  problem  [28],  which  is  then  solved 
using  the  interior  point  method  [29] . 

4  SIMULATION  RESULTS  AND  DISCUSSION 

In  this  section,  numerical  simulations  of  the  developed  nonlinear 
MPC  obstacle  avoidance  algorithm  with  the  14  DoF  vehicle 
model  mentioned  in  Section  3.1  as  the  AGV  shown  in  Fig.  1  are 
presented. 

Two  scenarios  are  considered  with  two  different  obstacle 
fields,  and  each  scenario  is  simulated  with  both  the  formulation 
presented  in  this  paper  and  the  constant  speed  formulation  of  [19] 
for  comparison.  All  simulations  are  run  with  a  vehicle  initial 
speed  of  20  m/s.  The  LIDAR  detection  range  is  Rlidar  =  100 
m.  The  goal  is  to  pass  through  the  specified  target  from  the 
90  degrees  direction  in  the  global  coordinate.  At  each  step  of 
the  MPC,  the  optimization  generates  a  trajectory  within  the  100 
m  LIDAR  detection  range.  Only  the  first  0.5  second  of  the 
planned  control  command  is  executed  by  the  vehicle.  A  new 
trajectory  is  planned  every  0.5  second  using  the  updated  vehicle 
state  information  and  obstacle  information  from  the  sensors. 

The  results  of  the  first  scenario  are  shown  in  Fig.  13.  In 
this  case,  both  the  constant  speed  algorithm  and  variable  speed 
algorithm  navigate  the  vehicle  through  the  obstacle  held  safely; 
i.e.,  collision-free  as  shown  in  Fig.  13a  and  without  tire  lift-off 
as  shown  in  Fig.  13d  and  Fig.  13e.  In  the  constant  speed  case, 
because  the  speed  controller  used  does  not  take  the  steering  input 
into  account,  the  speed  has  a  maximum  deviation  of  0.3  m/s  from 
the  desired  value  of  20  m/s.  In  the  variable  speed  case,  the  speed 
of  the  vehicle  gradually  increases  from  20  m/s  up  to  22  m/s  and 
decreases  back  towards  20  m/s  within  the  final  3.5  seconds  as 
desired.  In  this  case,  the  vehicle  arrives  the  target  1.7  seconds 
earlier  out  of  the  25  seconds  trajectory;  i.e.,  about  7%  faster. 
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(a)  Trajectory. 


(d)  Vertical  load  for  the  constant 
speed  case. 


(b)  Longitudinal  speed. 


(c)  Steering  angle. 


(e)  Vertical  load  for  the  variable 
speed  case. 


FIGURE  13.  Results  of  simulation  set  1.  Both  constant  speed  and 
variable  speed  navigations  are  successful.  In  the  variable  speed  case, 
the  vehicle  arrives  the  target  earlier.  In  the  subplots  (d)  and  (e),  the 
black  dashed  lines  indicate  the  minimum  allowable  vertical  tire  force. 


(a)  Trajectory. 


Constant  U 


(b)  Longitudinal  speed. 


Constant  U 


(c)  Steering  angle. 


x[m] 


(d)  Trajectory  zoom  in  view. 


(e)  Vertical  load  for  the  constant 
speed  case. 


(f)  Vertical  load  for  the  variable 
speed  case. 


A  more  important  benefit  of  the  new  formulation  is 
demonstrated  with  the  second  scenario  as  shown  in  Fig.  14. 
With  the  constant  speed  algorithm,  the  vehicle  collides  with  the 
obstacle  after  11.5  seconds.  However,  with  the  variable  speed 
algorithm,  the  vehicle  is  safely  navigated  through  the  obstacle 
field  by  decelerating  to  a  speed  around  10  m/s.  The  vehicle  starts 
accelerating  again  after  the  obstacle  is  cleared.  As  shown  in 
Fig.  14a  and  Fig.  14d,  the  trajectory  is  collision  free.  Fig.  14f 
shows  that  the  tire  vertical  loads  are  all  above  the  specified 
threshold;  hence,  dynamical  safety  of  the  vehicle  is  ensured,  as 
well. 

5  SUMMARY  AND  CONCLUSIONS 

This  paper  considers  AG  Vs  in  unstructured  environments 
without  a  priori  information  about  the  obstacles  and  presents 
an  MPC-based  obstacle  avoidance  algorithm  that  optimizes  the 
longitudinal  speed  and  steering  angle  simultaneously  to  navigate 
the  AGV  safely  and  as  quickly  as  possible  to  the  target  location. 


FIGURE  14.  Results  of  simulation  set  2.  The  constant  speed 
navigation  fails  while  the  variable  speed  navigation  is  successful. 

A  multi-stage  OCP  formulation  is  used  to  incorporate  the 
obstacle  data  obtained  from  the  on-board  LIDAR  sensor.  The 
formulation  of  the  OCP  to  be  solved  at  each  step  of  the  MPC  is 
discussed  in  detail.  The  powertrain  and  brake  dynamics  are  taken 
into  consideration  through  the  bounds  on  vehicle  longitudinal 
speed,  acceleration  and  jerk.  The  dynamical  safety  requirement 
is  also  accounted  for  by  enforcing  a  positive  vertical  load  on  all 
four  tires.  Two  sets  of  numerical  simulations  are  conducted  to 
demonstrate  the  effectiveness  of  the  algorithm.  The  conclusion 
is  that  the  developed  variable  speed  algorithm  not  only  improves 
the  performance  of  the  vehicle  by  allowing  it  to  operate  closer 
to  its  dynamical  limits,  but  also  enables  the  safe  clearance  of 
obstacle  fields  that  may  not  be  cleared  with  steering  control 
alone. 
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Future  research  directions  include  investigation  of 
alternative  cost  function  formulations  and  systematic  ways 
of  selecting  the  weighting  factors.  Incorporation  of  uncertainties 
in  the  model  and  sensor  measurements  is  another  direction. 
Although  the  prediction-correction  characteristic  allows  the 
algorithm  to  tolerate  some  errors  and  uncertainties,  incorporating 
them  explicitly  would  further  increase  the  robustness  of  the 
algorithm.  Finally,  extension  of  the  formulation  to  moving 
obstacles  is  also  an  important  direction  for  future  research. 
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